//
// Created by xuwj on 2021/11/10.
//

#include <iostream>
#include <memory>

#include "opencv2/opencv.hpp"

#include "PointcloudProcessing/FilterUtilities.h"

using namespace std;
using namespace CV_Assistant;

const string config_file = "/home/xuwj/projects/CV_Assistant/configs/PointcloudProcessing/params_PCproc.yaml";

typedef pcl::PointXYZRGBA point_type;
typedef pcl::PointCloud<point_type> cloud_type;
typedef boost::shared_ptr<cloud_type> cloud_Ptr;

int main() {
    cv::FileStorage fSettings(config_file,CV_STORAGE_READ);
    if (!fSettings.isOpened()){
        cout<<"\nError with your config file's path!!!"<<endl;
    }

    const string pcd_file = fSettings["PCDFilePath"];
    const string pcd_output_path = fSettings["PCDOutputPath"];
    const string map_output_path = fSettings["ImageOutputPath"];

    //离群点
    const int param_meanK = fSettings["Statistical.meanK"];
    const double param_StdDevMulThresh = fSettings["Statistical.StdDevMulThresh"];

    //HSV筛选
    const int param_Hmin = fSettings["HSV.Hmin"];
    const int param_Hmax = fSettings["HSV.Hmax"];
    const double param_Smin = fSettings["HSV.Smin"];
    const double param_Smax = fSettings["HSV.Smax"];
    const double param_Vmin = fSettings["HSV.Vmin"];
    const double param_Vmax = fSettings["HSV.Vmax"];

    //平面分割
    const double param_PlaneSeg_disth = fSettings["PlaneSeg.DisThresh"];

    //选定轴剪切
    const string param_cut_axis = fSettings["cut_axis"];
    const double param_cut_floor = fSettings["cut_floor"];
    const double param_cut_ceil = fSettings["cut_ceil"];

    //投影至平面
    float param_plane_coeffs[4];
    param_plane_coeffs[0] = fSettings["ProjCoeffs.0"];
    param_plane_coeffs[1] = fSettings["ProjCoeffs.1"];
    param_plane_coeffs[2] = fSettings["ProjCoeffs.2"];
    param_plane_coeffs[3] = fSettings["ProjCoeffs.3"];

    //转栅格图
    const int param_image_border = fSettings["ImageBorder"];
    const int param_occupied_thresh = fSettings["OccupiedThresh"];
    const double param_map_resolution = fSettings["MapResolution"];
    const int yes_value = fSettings["YesValue"];
    const int no_value = fSettings["NoValue"];
    const int map_max_width = fSettings["MapMaxWidth"];
    const int map_max_height = fSettings["MapMaxHeight"];

    PointcloudProcessing::FiltersHelper<point_type> filter_helper(pcd_file);

    /**
     * HSV筛选
     */
//    PointcloudProcessing::FilterByHSV(filter_helper.GetCurrentCloudPtr(), param_Hmin, param_Hmax);


    /**
     * 可视化原始点云
     */
//    filter_helper.VisualizeCloud(filter_helper.GetCurrentCloudPtr());

    /**
     *  剔除离群点
     */
    filter_helper.RemoveOutlier_Statistical(param_meanK,param_StdDevMulThresh,false);

    /**
     *  RANSAC分割平面
     */
//    Eigen::Vector4d plane_coef = filter_helper.SinglePlaneSegmentation(filter_helper.GetCurrentCloudPtr(),param_PlaneSeg_disth,true);

    /**
     * 展示并选择是否删除 选定轴 指定范围 以外的所有点
     */
//    filter_helper.SelectRangeShowOnAxis(filter_helper.GetCurrentCloudPtr(),param_cut_axis,param_cut_floor,param_cut_ceil);

    /**
     * 输入三轴坐标范围， 展示并选择是否删除 范围内的点
     */
//    filter_helper.SelectedAreaCut(filter_helper.GetCurrentCloudPtr());

    /**
     * 将点云投影到 一个确定参数的平面上 ，默认为y=1的平面
     */
//    filter_helper.ProjectToPlane(filter_helper.GetCurrentCloudPtr(),param_plane_coeffs,false);

    /**
     * 保存点云，写为PCD文件
     */
    filter_helper.WriteToPCD(filter_helper.GetCurrentCloudPtr(),pcd_output_path+"rmStats.pcd");

    /**
     * 将点云 按每一格的点数是否达到规定阈值， 转化为二值图像
     */
//    cv::Mat map;
//    filter_helper.CloudToGridImage(filter_helper.GetCurrentCloudPtr(),map,"x","z",
//                                   param_occupied_thresh,map_output_path,param_image_border,
//                                   param_map_resolution, yes_value, no_value,
//                                   map_max_width,map_max_height);

    return 0;
}


